/*************************************************************************
*
*  compass.c
*
*
*  Copyright 2008 Steve Stancliff
*
*
*  Part of the TAOSF SSA system.
*
*
*  This module reads gps data from a gpsd server.
*
*
*  version 0.1, 11/19/08, SBS
*   - first skeletal version 
*
**************************************************************************
*
*  This program is free software; you can redistribute it and/or modify
*  it under the terms of the GNU General Public License as published by
*  the Free Software Foundation; either version 2 of the License, or
*  (at your option) any later version.
*
*  This program is distributed in the hope that it will be useful, but 
*  WITHOUT ANY WARRANTY; without even the implied warranty of 
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
*  General Public License for more details.
*
*  You should have received a copy of the GNU General Public License 
*  along with this program; if not, write to the Free Software Foundation, 
*  Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 
*
**************************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>

#include "logfile.h"
#include "delay.h"

#include "cssl.h"


typedef struct{
	float hdgDeg;
	float pitchDeg;
	float rollDeg;
	float tempDegC;
}COMPASS_STATUS;


static COMPASS_STATUS cs;
static cssl_t *serial;


/*************************
*
*
*************************/
void compassInterruptCallback(int signum)
{
	#ifdef DEBUG
	char str[LOGSTRLEN];

	snprintf(str,LOGSTRLEN, "\nExiting on signal %d!", signum);
	logfile_LogPrint(str);
	logfile_CloseFile();
	#endif

	printf("\nExiting on signal %d!", signum);
	cssl_close(serial);
	cssl_stop();
	exit(signum);
}


/*************************
*
*
*************************/
static void compassSetIntCallback()
{
	signal(SIGINT, compassInterruptCallback);
	signal(SIGTERM, compassInterruptCallback);
	signal(SIGQUIT, compassInterruptCallback);
	signal(SIGHUP, compassInterruptCallback);
}


/*************************
*
*
*************************/
static void parse_compass(char *buf)
{
	char *pS;
	int rval;

// need to add verification of checksum

	pS = strrchr(buf,'$');
	if(pS == NULL){
		printf("No '$' found\n");
		return;
	}

	rval = sscanf(pS
				,"$C%fP%fR%fT%f*"
				,&(cs.hdgDeg)
				,&(cs.pitchDeg)
				,&(cs.rollDeg)
				,&(cs.tempDegC)
				);
				
	if(rval == EOF){
		printf("Incomplete string\n");
		return;
	}

	return;
}


/*************************
*
*
*************************/
void compass_callback(
			int id,
			uint8_t *buf,
			int length)
{
	if(strchr((char *)buf,'$') != NULL){
		parse_compass((char *)buf);
	}

	return;
}


/*************************
*
*
*************************/
static void compassOpenLogfile(
			char *logfilename)
{
	#ifdef DEBUG
	logfile_OpenFile(logfilename);
	logfile_LogLevelSet(LOGLEVEL_DEBUG);
	logfile_LogPrint(" ");
	logfile_LogPrint("*******************************************");		
	logfile_LogPrint("COMPASS STARTUP");
	logfile_LogPrint("*******************************************");		
	logfile_LogPrint(" ");
	#endif

	return;
}


/*************************
*
*
*************************/
static int compassInit(
			char *port
			,int baud
			,int bits
			,int stopbits)
{
	cssl_start();
    
	serial = cssl_open(
				"/dev/compass0"
				,compass_callback
				,0
				,baud
				,bits
				,0
				,1);
	
	if (!serial) {
		printf("%s\n",cssl_geterrormsg());
		return(-1);
	}

	return(0);
}


/*************************
*
*
*************************/
static void mainloop()
{	
	TIMESPEC delay;
	char	logstr[LOGSTRLEN];
	
	delay = delay_setdelay(2.0);
	
	while(1){
	
		snprintf(logstr,LOGSTRLEN,"HPRT (float) = %0.1f %0.1f %0.1f %0.1f"
				,cs.hdgDeg
				,cs.pitchDeg
				,cs.rollDeg
				,cs.tempDegC);
		logfile_LogPrint(logstr);

//		if(DEBUGSCREEN){				
		printf(".");
		fflush(stdout);
		
		delay_sleep(&delay);
	}	
	
	return;
}


/*************************
*
*
*************************/
int main(int argc, char **argv)
{
	char *port = "/dev/ttyUSB0";
	int baud = 19200;
	int bits = 8;
	int stopbits = 1;
	
	compassOpenLogfile("logfile.log");
		
	compassSetIntCallback();

	compassInit(port,baud,bits,stopbits);
	
	mainloop();

	return(0);
}

